
#include <Wire.h>                //This library allows you to communicate with I2C / TWI devices.
#include <string.h>
#include <Servo.h>

#undef int
#include <stdio.h>

Servo right_servo;
Servo left_servo;
int cw;
int ccw;
int kill_switch = 90;

uint8_t outbuf[6];		// array to store arduino output
int cnt = 0;
int ledPin = 13;

void setup ()
{
  Serial.begin (19200);
  Serial.print ("Finished setup\n");
  Wire.begin ();		// join i2c bus with address 0x52
  nunchuck_init (); // send the initilization handshake
  right_servo.attach(9);
  left_servo.attach(10);
}

void nunchuck_init ()
{
  Wire.beginTransmission (0x52);	// transmit to device 0x52
  Wire.write (0x40);		// sends memory address
  Wire.write (0x00);		// sends sent a zero.  
  Wire.endTransmission ();	// stop transmitting
}

void send_zero ()
{
  Wire.beginTransmission (0x52);	// transmit to device 0x52
  Wire.write (0x00);		// sends one byte
  Wire.endTransmission ();	// stop transmitting
}

void loop ()
{
  Wire.requestFrom (0x52, 6);	// request data from nunchuck
  while (Wire.available ())
    {
      outbuf[cnt] = nunchuk_decode_byte (Wire.read ());	// receive byte as an integer
      digitalWrite (ledPin, HIGH);	// sets the LED on
      cnt++;
    }

  // If we recieved the 6 bytes, then go print them
  if (cnt >= 5)
    {
      print ();
    }

  cnt = 0;
  send_zero (); // send the request for next bytes
  delay (100);
}

// Print the input data we have recieved
// accel data is 10 bits long
// so we read 8 bits, then we have to add
// on the last 2 bits.  That is why I
// multiply them by 2 * 2
void print ()
{
  static int i=0;
  int joy_x_axis = outbuf[0];
  int joy_y_axis = outbuf[1];
  int accel_x_axis = outbuf[2]; 
  int accel_y_axis = outbuf[3];
  int accel_z_axis = outbuf[4];

  int z_button = 0;
  int c_button = 0;
  ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 
  // Angle for Accelerometer
  int mid_accel = 129;
  int max_accel = 258;
  
  int fx = (accel_x_axis - mid_accel);
  int fy = (accel_y_axis - mid_accel);
  int fz = (accel_z_axis - mid_accel);
  
  float ax = fx*(3.3/(1024.0*((max_accel))/180));  //The 3.3V supply volt is divided by 1024 steps from the A/D converter. This value is divided by the sensitivity in X axis
  float ay = fy*(3.3/(1024.0*((max_accel))/180));  //The 3.3V supply volt is divided by 1024 steps from the A/D converter. This value is divided by the sensitivity in Y axis
  float az = fz*(3.3/(1024.0*((max_accel))/180));  //The 3.3V supply volt is divided by 1024 steps from the A/D converter. This value is divided by the sensitivity in Z axis
  
  float rad_to_degrees = (360/(2*3.14159));  //converts radians to degrees
  
  float rho =   atan(ax/sqrt(pow(ay,2)+pow(az,2))) * rad_to_degrees;
  float phi =   atan(ay/sqrt(pow(ax,2)+pow(az,2))) * rad_to_degrees;
  float theta = atan(sqrt(pow(ay,2)+pow(ax,2))/az) * rad_to_degrees;
  
  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  // Angle for Joystick
  int joy_min = 25;
  int joy_max = 225;
  int joy_mid = (joy_min + joy_max) / 2;
  
  int joy_x_centered = joy_x_axis - joy_mid;
  int joy_y_centered = joy_y_axis - joy_mid;
 
  float max_angle = 30;  //degrees
  
  float degree = (max_angle / (joy_max - joy_mid));
  
  float x_degree = degree * joy_x_centered;
  float y_degree = degree * joy_y_centered;
  ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 
  
 // byte outbuf[5] contains bits for z and c buttons
 // it also contains the least significant bits for the accelerometer data
 // so we have to check each bit of byte outbuf[5]
  if ((outbuf[5] >> 0) & 1)
    {
      z_button = 1;
    }
  if ((outbuf[5] >> 1) & 1)
    {
      c_button = 1;
    }

  if ((outbuf[5] >> 2) & 1)
    {
      accel_x_axis += 2;
    }
  if ((outbuf[5] >> 3) & 1)
    {
      accel_x_axis += 1;
    }

  if ((outbuf[5] >> 4) & 1)
    {
      accel_y_axis += 2;
    }
  if ((outbuf[5] >> 5) & 1)
    {
      accel_y_axis += 1;
    }

  if ((outbuf[5] >> 6) & 1)
    {
      accel_z_axis += 2;
    }
  if ((outbuf[5] >> 7) & 1)
    {
      accel_z_axis += 1;
    }
  
  Serial.print (i,DEC);
  Serial.print ("     ");
  
  Serial.print ("Joy x: ");
  Serial.print (joy_x_axis, DEC);
  Serial.print ("   Angle: ");
  Serial.print (x_degree, 2);
  
  Serial.print ("     y: ");
  Serial.print (joy_y_axis, DEC);
  Serial.print ("   Angle: ");
  Serial.print (y_degree, 2);
  
  Serial.print ("     Accel x: ");
  Serial.print (accel_x_axis, DEC);
  Serial.print ("   Angle: ");
  Serial.print (rho, 2);

  Serial.print ("     y: ");
  Serial.print (accel_y_axis, DEC);
  Serial.print ("   Angle: ");
  Serial.print (phi, 2);
  
  Serial.print ("     z: ");
  Serial.print (accel_z_axis, DEC);
  Serial.print ("   Angle: ");
  Serial.print (theta, 2);
  
  Serial.print ("     Button z: "); 
  Serial.print (z_button, DEC);
  
  Serial.print ("     c: ");
  Serial.print (c_button, DEC);
 
  Serial.print ("\r\n");
  i++;  
  
  if (z_button == 1) {
    cw = kill_switch;
    ccw = kill_switch;
  }
  
  else if (z_button == 0) {
    cw = 180;
    ccw = -180;
  }
  
  if (c_button == 0) {
    
    if (accel_y_axis >= 150) {
      right_servo.write(cw);
      left_servo.write(ccw);
      delay(10);
    }
    else if (accel_y_axis <= 100) {
      right_servo.write(ccw);
      left_servo.write(cw);
      delay(10);
    }
    else if (accel_y_axis > 100 && accel_y_axis < 150) {
      right_servo.write(kill_switch);
      left_servo.write(kill_switch);
      delay(10);
    }
    
    if (accel_x_axis <= 100) {
      right_servo.write(cw);
      left_servo.write(kill_switch);
      delay(10);
    }
    if (accel_x_axis >= 150) {
      left_servo.write(ccw);
      right_servo.write(kill_switch);
      delay(10);
    }
  }
  
  else if (c_button == 1) {
    
    if (joy_y_axis >= 175) {
      right_servo.write(cw);
      left_servo.write(ccw);
      delay(10);
    }
    else if (joy_y_axis <= 75) {
      right_servo.write(ccw);
      left_servo.write(cw);
      delay(10);
    }
    else if (joy_y_axis > 75 && joy_y_axis < 175) {
      right_servo.write(kill_switch);
      left_servo.write(kill_switch);
      delay(10);
    }
    
    if (joy_x_axis <= 75) {
      right_servo.write(cw);
      left_servo.write(kill_switch);
      delay(10);
    }
    if (joy_x_axis >= 175) {
      left_servo.write(ccw);
      right_servo.write(kill_switch);
      delay(10);
    }
  }  
}

// Encode data to format that most wiimote drivers except
// only needed if you use one of the regular wiimote drivers
char nunchuk_decode_byte (char x)
{
  x = (x ^ 0x17) + 0x17;
  return x;
}
